#include "costmap_compressor_core.h"

// 压缩节点
CostmapCompressor::CostmapCompressor():nh_(""), private_nh_("~") {
    private_nh_.param<std::string>("input_costmap_compress_topic", input_costmap_compress_topic, "");
    private_nh_.param<std::string>("output_costmap_compress_topic", output_costmap_compress_topic, "");
}

CostmapCompressor::~CostmapCompressor(){}

void CostmapCompressor::compress_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
    costmap_compress::CompressedOccupancyGrid compressed_msg;
    compressed_msg.header = msg->header;
    compressed_msg.info = msg->info;
    compressed_msg.original_length = msg->data.size();

    // 位压缩核心算法
    uint8_t current_byte = 0;
    int bit_counter = 0;
    int obstacle_count = 0;
    for (const auto& val : msg->data) {
        bool bit = (val != 0);
        if(bit)
        {
            obstacle_count++;
        }
        current_byte |= (bit << bit_counter); // 低位优先存储
        if (++bit_counter == 8) {
            compressed_msg.compressed_data.push_back(current_byte);
            current_byte = 0;
            bit_counter = 0;
        }
    }
    // 处理剩余位数
    if (bit_counter > 0) {
        compressed_msg.compressed_data.push_back(current_byte);
    }
    // 打印压缩后的数据长度和障碍点数量
    // ROS_INFO("Compressed data size: %zu, Obstacle count: %d", compressed_msg.compressed_data.size(), obstacle_count);

    //pub_.publish(compressed_msg);
    std::lock_guard<std::mutex>lock(msg_mytex_);
    latest_msg_=compressed_msg;
    has_new_data_=true;
}

void CostmapCompressor::process(const ros::TimerEvent& e){
    std::lock_guard<std::mutex>lock(msg_mytex_);
    if(has_new_data_){
        costmap_compress_pub_.publish(latest_msg_);
        has_new_data_=false;
    }
}

// 解压节点
CostmapDecompressor::CostmapDecompressor():nh_(""), private_nh_("~") {
    private_nh_.param<std::string>("input_costmap_decompress_topic", input_costmap_decompress_topic, "");
    private_nh_.param<std::string>("output_costmap_decompress_topic", output_costmap_decompress_topic, "");
}

CostmapDecompressor::~CostmapDecompressor(){}

void CostmapDecompressor::decompress_callback(const costmap_compress::CompressedOccupancyGrid::ConstPtr& msg) {
    nav_msgs::OccupancyGrid decompressed_msg;
    decompressed_msg.header = msg->header;
    decompressed_msg.info = msg->info;
    decompressed_msg.data.resize(msg->original_length, 0);

    size_t idx = 0;
    for (const auto& byte : msg->compressed_data) {
        for (int i=0; i<8 && idx<msg->original_length; ++i) {
            bool bit = (byte >> i) & 0x01; // 低位优先解包
            decompressed_msg.data[idx++] = bit ? 100 : 0;
        }
    }
    costmap_pub_.publish(decompressed_msg);
}

void CostmapCompressor::run(){
    costmap_sub_ = nh_.subscribe<nav_msgs::OccupancyGrid>(input_costmap_compress_topic, 1, &CostmapCompressor::compress_callback, this);
    costmap_compress_pub_ = nh_.advertise<costmap_compress::CompressedOccupancyGrid>(output_costmap_compress_topic, 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.2), &CostmapCompressor::process, this, false, true);
    ros::spin();
}


void CostmapDecompressor::run(){
    costmap_decompress_sub_ = nh_.subscribe<costmap_compress::CompressedOccupancyGrid>(input_costmap_decompress_topic, 1, &CostmapDecompressor::decompress_callback, this);
    costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(output_costmap_decompress_topic, 1);
    ros::spin();
}


